/***************************************************************************
 创建者: 华磊
 开始时间: 2017.6.28
 copyright: (C) 华友高科
 修改说明:
 ***************************************************************************
 *  @file axisobject.h
 *  功能:
 * 轴对象的管理和对外接口。
 *
 *                                                                         *
 ***************************************************************************/
#include "axisobject.h"
#include "qdebug.h"
#include "filteringalgorithm.h"
#include "velocityprofile_improvedspline.h"
#include "velocityprofile_sin.h"
#include "simple_tp.h"

#define D_SPLINE_TIME_ESTIMATE_RATIO 2.6 //
#define D_DEBUG_AXIS_POSITION 0

AxisObject::AxisObject(int robotIdIn, JointAttribute axisAttributeIn, MotionMessage *motionMessageIn)
{
    robotId=robotIdIn;
    motionMessage=motionMessageIn;
//    velProfile=NULL;
    timeAxisScale=1.0;
    deltaTime=1.0;
    axisAbstractType=E_AXIS_ABSTRACT_VIRTUAL;
    axisIdByRobot=0;
    loadAxisAttribute(axisAttributeIn);
    currentRunningLine=-1;
    iscurrentAxisCommandFInished=-1;
    currentCommandRemainingTime=-1;
    totalRemainingTime=-1;
    linesNumberQueue.clear();
    linesDurationQueue.clear();
    positionCommand=0.0;
    positionCommandOld=0.0;
    velCommand=0.0;
    accCommand=0.0;
    torqueCommand=0.0;
    positionFeedback=0.0;
    positionFeedbackOld=0.0;
    velFeedback=0.0;
    torqueFeedback=0.0;
    positionFollowOffset=0.0;
    velFollowOffset=0.0;
    torqueFollowOffset=0.0;
    axisAccelerationRatio=1.0;
//    accumulatedTime=0.0;
    velCompensation=0;
    torqueCompensation=0;
    basicStatus=E_AXIS_STATUS_NORMAL;
    limitCheckMode=E_CHECK_MODE_T2;
    isPositionCommandExceed=0;
    isVelCommandExceed=0;
    isAccCommandExceed=0;
    isTorqueCommandExceed=0;
    isProcessFollowErrorExceed=0;
    controlMode=E_MOTOR_CONTROL_CSP_MODE;
    controlMode_old=E_MOTOR_CONTROL_CSP_MODE;
    velCommand_byIncrement=0;
    axisPlanMode=E_AXIS_PLAN_MODE_COMMON;

//    for(int i=0;i<VEL_PROFILE_ARRY_MAX;i++)
//    {
//        velProfileArry[i]=NULL;
//    }
//    timeDurationCache.clear();
//    currentVelProfileArryIndex=-1;
    filteringAlgorithm=new FilteringAlgorithm;
    filteringAlgorithm_qqvel=new FilteringAlgorithm;
    filteringAlgorithm_position=new FilteringAlgorithm;
    simpleTp=new simple_tp_t;
    simpleTp->active=0;
    resetAll();

}

int AxisObject::axisSimpleLoopOnce()
{
    if(E_AXIS_PLAN_MODE_TP!=axisPlanMode)
    {
        return 0;
    }
    simple_tp_update(simpleTp,deltaTime);
    positionCommand=simpleTp->curr_pos;
    velCommand=simpleTp->curr_vel;

    return isLimitCheckOk();;
}

int AxisObject::getAxisSimpleTpInfo(SimpleTpInfo &infoOut)
{
    infoOut.active=simpleTp->active;
    infoOut.curr_pos=simpleTp->curr_pos;
    infoOut.curr_vel=simpleTp->curr_vel;
    infoOut.enable=simpleTp->enable;
    infoOut.max_acc=simpleTp->max_acc;
    infoOut.max_vel=simpleTp->max_vel;
    infoOut.pos_cmd=simpleTp->pos_cmd;
    return 1;
}

int AxisObject::controlAxisSimpleTp(int isEnable)
{
    simpleTp->enable=isEnable;
    return 1;
}

int AxisObject::setAxisSimpleTpTarget(double targetPosIn, double maxVelIn, double maxAccIn)
{
    simpleTp->pos_cmd=targetPosIn;
    if(fabs(maxVelIn)>maxVelocity)
    {
        simpleTp->max_vel=maxVelocity;
    }
    else
    {
        simpleTp->max_vel=maxVelIn;
    }

    if(fabs(maxAccIn)>maxAccelation)
    {
        simpleTp->max_acc=maxAccelation;
    }
    else
    {
        simpleTp->max_acc=maxAccIn;
    }
    return 1;
}

E_AXIS_PLAN_MODE AxisObject::getAxisPlanMode()
{
    return axisPlanMode;
}

int AxisObject::setAxisPlanMode(E_AXIS_PLAN_MODE modeIn)
{
    switch(modeIn)
    {
    case E_AXIS_PLAN_MODE_TP:
    {
        if(E_AXIS_PLAN_MODE_TP!=axisPlanMode)
        {
            resetSimpleTp();
        }
    }
    }
    axisPlanMode=modeIn;
    return 1;
}

int AxisObject::ceaseAxisObject()
{
    controlAxisSimpleTp(0);
    return 1;
}

double AxisObject::getIdealTorque()
{
    return idealTorque;
}

int AxisObject::setIdealTorque(double torqueIn)
{
    idealTorque=torqueIn;
    return 1;
}

int AxisObject::setT1Ratio(double ratioIn)
{
    t1RatioLimit=ratioIn;
    return 1;

}

int AxisObject::setPlanStartPosition_byPositionCommand()
{
    positionCommand_planed=positionCommand;
    return 1;
}

int AxisObject::setPlanStartPosition(double positionIn)
{
    positionCommand_planed=positionIn;
    return 1;
}

double AxisObject::getMixDurationLeft()
{
    if(E_MIX_STAGE_NONE==mixControlInfo.stage)
    {
        return -1;
    }
    return mixControlInfo.segmentDurationLeft;
}

double AxisObject::getPositionCommand_filter()
{
    return positionCommand;
//     filtering_interpolationRatio=0.1;
//     filtering_maxJerkBias=0.001;
//     return filteringAlgorithm_position->filterByForwardVel(positionCommand,filtering_interpolationRatio,filtering_maxJerkBias);
    //     return filteringAlgorithm_position->arithmeticMeanfilter(positionCommand,20);
}

double AxisObject::getVelLimitRatio()
{
    return fabs(velCommand_byIncrement/maxVelocity);
//    return fabs(velFeedback/maxVelocity);//滞后了。
//    return fabs(velCommand/maxVelocity);
}

double AxisObject::getStopTime()
{
    double tmpTime=fabs(velFeedback/maxAccelation);
    if(tmpTime<deltaTime)
    {
        return deltaTime;
    }
    return tmpTime;
}

int AxisObject::setAxisProperty(AxisProperty_hmi axisPropertyIn)
{
    minPosition=axisPropertyIn.minPosition;
    maxPosition=axisPropertyIn.maxPosition;
    approachDistance=axisPropertyIn.approachDistance;
    maxVelocity=axisPropertyIn.maxVelocity;
    maxFeedbackVelocity=axisPropertyIn.maxFeedbackVelocity;
    maxAccelation=axisPropertyIn.maxAccelation;
    processVelocityFollowError=axisPropertyIn.processVelocityFollowError;
    processFollowError=axisPropertyIn.processFollowError;
    targetFollowError=axisPropertyIn.targetFollowError;
    jogTorqueLimit=axisPropertyIn.jogTorqueLimit;
//    motorRatedTorque=axisPropertyIn.motorRatedTorque;
//    motorTorqueLimitPositive;
//    motorTorqueLimitNegative;
    maxTorque=axisPropertyIn.maxTorque;
    return 1;
}

int AxisObject::setAxisPositionLimit(double minPositionIn, double maxPositionIn,double approachDistanceIn)
{
    minPosition=minPositionIn;
    maxPosition=maxPositionIn;
    approachDistance=approachDistanceIn;
    return 1;
}


int AxisObject::setMaxVel(double maxVelIn)
{
    maxVelocity=maxVelIn;
    return 1;
}

int AxisObject::setAccelerationRatio(double ratioIn)
{
    qDebug()<<"AxisObject::setAccelerationRatio old"<<axisAccelerationRatio<<"new"<<ratioIn;
    if(ratioIn>0)
    {
        axisAccelerationRatio=ratioIn;
    }

    return 1;

}

int AxisObject::setMaxAcceleration(double maxAccIn)
{

    maxAccelation=maxAccIn;
    return 1;
}

double AxisObject::getVelCompensation()
{
    double tmpValue=filteringAlgorithm_qqvel->arithmeticMeanfilter(velCommand_byIncrement,6);
//    double tmpValue=velCommand_byIncrement;同步跟踪时，皮带速度不够平滑
    return velCompensation+tmpValue;
}

int AxisObject::setServoOnStatus(bool isOn)
{
    isServoOnStatus=isOn;
    //
    if(false==isOn)
    {
        positionCommand=positionFeedback;
        positionCommandOld=positionCommand;

    }
    return 1;
}

int AxisObject::axisFeedbackCheck()
{

    if(0)
    {
        qDebug()<<"axisFeedbackCheck axisId"<<axisIdByRobot<<"positionCommandOld"<<positionCommandOld
                <<"positionCommand"<<positionCommand;
    }
    //计算增量值
    positionFollowOffset = positionCommand-positionFeedback;
    velCommand_byIncrement=(positionCommand-positionCommandOld)/deltaTime;
    positionCommandOld=positionCommand;//更新旧值
    positionFeedbackOld=positionFeedback;

    if(E_CHECK_EMERGENCY_STOP==limitCheckMode)
    {
        return 1;
    }

    if(E_MOTOR_CONTROL_HOMING==controlMode)
    {
        return 1;
    }

    switch(axisLogicalType)
    {
    case E_AXIS_LOGIC_TYPE_AGV:
        {
            if(1==isVelFeedbackCheckOk(velFeedback)
                    && 1==isTorqueFeedbackCheckOk(torqueFeedback))
            {
               return 1;
            }
            else
            {
                return -2;
            }
        }
        break;
    case E_AXIS_LOGIC_TYPE_AGVSTEERING:
        {
        if(isServoOnStatus)
        {
            if(1==isPositionFeedbackCheckOk(positionFeedback)
                    && 1==isVelFeedbackCheckOk(velFeedback,false)
                    && 1==isTorqueFeedbackCheckOk(torqueFeedback)
                    && 1==isVelFeedbackIncrementCheckOk(velFeedback)
                    && 1==isPositionFollowOffsetCheckOk()
                    && 1==isVelocityFollowOffsetOk())
            {
                return 1;
            }
            else
            {
                return -1;
            }
        }
        else
        {
            if(1==isPositionFeedbackCheckOk(positionFeedback)
                    && 1==isVelFeedbackCheckOk(velFeedback,false)
                    && 1==isTorqueFeedbackCheckOk(torqueFeedback)
                    && 1==isVelFeedbackIncrementCheckOk(velFeedback))
            {
                return 1;
            }
            else
            {
                return -1;
            }
        }
        }
        break;
//    case E_AXIS_LOGIC_TYPE_VIRTUAL_STAND_ALONE:
//        {
//               return 1;

//        }
//        break;
    default:
    {
        if(isServoOnStatus)
        {
            if(1==isPositionFeedbackCheckOk(positionFeedback)
                    && 1==isVelFeedbackCheckOk(velFeedback)
                    && 1==isTorqueFeedbackCheckOk(torqueFeedback)
                    && 1==isVelFeedbackIncrementCheckOk(velFeedback)
                    && 1==isPositionFollowOffsetCheckOk()
                    && 1==isVelocityFollowOffsetOk())//跟踪模式会导致偏差很大。
            {
                return 1;
            }
            else
            {
                return -1;
            }
        }
        else
        {
            if(1==isPositionFeedbackCheckOk(positionFeedback)
                    && 1==isVelFeedbackCheckOk(velFeedback)
                    && 1==isTorqueFeedbackCheckOk(torqueFeedback)
                    && 1==isVelFeedbackIncrementCheckOk(velFeedback))
            {
                return 1;
            }
            else
            {
                return -1;
            }
        }

    }
    }

    return -100;
}

int AxisObject::isPositionCommandApprochLimit()
{
    if(E_AXIS_LOGIC_TYPE_AGV==axisLogicalType)
    {
       return 0;
    }
    if((positionCommand > maxPosition-approachDistance) && ( positionCommand >positionCommandOld) )
    {
//       qDebug()<< "xxxxxxxxxxxxxxxxxx axid= "<< axisIdByRobot<<"  exceed maxPosition"
//                 <<maxPosition;
        addMsg(ENUM_MSG_WARNING,"MotionServer","AxisObject",5030,robotId, axisIdByRobot,positionCommand);
        return 1;
    }
    else if( (positionCommand < minPosition+approachDistance) && ( positionCommand <positionCommandOld) )
    {
//        qDebug()<< "xxxxxxxxxxxxxxxxxx axid= "<< axisIdByRobot<<"  exceed minPosition"
//                <<minPosition;
        addMsg(ENUM_MSG_WARNING,"MotionServer","AxisObject",5031,robotId, axisIdByRobot,positionCommand);

        return -1;
    }

    return 0;
}

int AxisObject::restoreControlMode()
{
    controlMode=controlMode_old;
    return 1;
}

int AxisObject::getMotorControlMode(E_MOTOR_CONTROL_MODE &modeOut)
{
    modeOut=controlMode;
    return 1;
}

int AxisObject::setMotorControlMode(E_MOTOR_CONTROL_MODE modeIn)
{
    controlMode=modeIn;
    return 1;
}

bool AxisObject::isTheLastTrajCommandMoving()
{
//    //最后一条velProfile不为空，-1==currentVelProfileArryIndex
//    if(NULL==velProfile && -1==currentVelProfileArryIndex)
//    {
//        return true;
//    }
//    else
//    {
//        return false;
//    }
    return isTrajComposeEmpty();
}


int AxisObject::isTrajComposeEmpty()
{
//    if(NULL==velProfile && -1==currentVelProfileArryIndex)
//    {
//        return 1;
//    }
//    else
//    {
//        return 0;
//    }

    if(0==trajAttributeQueue.size())
    {
        return true;
    }
    return false;
}

int AxisObject::deleteTrajComposite()
{
    int tmpSize=trajAttributeQueue.size();
    for(int i=0;i<tmpSize;i++)
    {
        delete trajAttributeQueue[i].trajComp;
    }
    trajAttributeQueue.clear();
    return 1;
}

VelocityProfile *AxisObject::getTrajComposite(int trajIndex)
{
    if(trajIndex<trajAttributeQueue.size() && trajIndex>=0)
    {
        return trajAttributeQueue[trajIndex].trajComp;
    }
    return NULL;
}


int AxisObject::addNewTrajComposite(AxisTrajeAttribute attrIn)
{
    attrIn.accumulatedTime=0;
    trajAttributeQueue.enqueue(attrIn);
    return 1;
}

int AxisObject::setTrajCompositeEndCutoffTime(QVector<double> timeIn)
{
    if(timeIn.size()!=trajAttributeQueue.size())
    {
        qDebug()<<"error333,,,,timeIn.size()!=trajAttributeQueue.size()";
        return -1;
    }
    for(int i=0;i<timeIn.size();i++)
    {
        trajAttributeQueue[i].endCutoffTime=timeIn[i];
    }
    return 1;
}


//int AxisObject::resetTraj()
//{
//    if(0)
//    {
//        qDebug()<<"AxisObject::resetTraj";
//    }

//    accumulatedTime=0;
////    if(false==isTrajComposeEmpty())
////    {
////        delete velProfile;
////        //velProfile->~VelocityProfile();
////        velProfile=NULL;
////    }

//    accumulatedTime_secondTraj=0;
//    deleteTrajComposite();

////    timeDurationCache.clear();
////    filteringAlgorithm->resetAll();
//    mixControlInfo.stage=E_MIX_STAGE_NONE;
//    return 1;
//}

int AxisObject::resetAll()
{
//    if(false==isTrajComposeEmpty())
//    {
//        delete velProfile;
//        //velProfile->~VelocityProfile();
//        velProfile=NULL;
//    }
//    currentVelProfileArryIndex=-1;
    clearTraj();
    positionCommand=positionFeedback;
    positionCommand_planed=positionCommand;
//    if(1==axisIdByRobot)
//    {
//        qDebug()<<"resetAll 1234positionCommand"<<positionCommand;
//    }
    positionCommandOld=positionCommand;
    positionFeedbackOld=positionFeedback;
    velCommand=0;
    torqueCommand=0;
    accCommand=0.0;
//    accumulatedTime=0.0;
    isPositionCommandExceed=0;
    isVelCommandExceed=0;
    isAccCommandExceed=0;
    isTorqueCommandExceed=0;
    isProcessFollowErrorExceed=0;
    velCompensation=0;
    torqueCompensation=0;
    basicStatus=E_AXIS_STATUS_NORMAL;
//    timeDurationCache.clear();
    filteringAlgorithm->resetAll();
    filteringAlgorithm_position->resetAll();
    maxErrorCount=0;
    minErrorCount=0;
    vel_old=0;
    mixControlInfo.stage=E_MIX_STAGE_NONE;
    axisAccelerationRatio=1;
    idealTorque=0;
    //tp
    resetSimpleTp();


    return 1;
}

int AxisObject::resetSimpleTp()
{
    simpleTp->pos_cmd=positionFeedback;
    simpleTp->max_vel=0;
    simpleTp->max_acc=0;
    simpleTp->enable=0;
    simpleTp->curr_pos=positionFeedback;
    simpleTp->curr_vel=0;
    return 1;
}


int AxisObject::calculateFollowOffset()
{
    positionFollowOffset = positionCommand-positionFeedback;
    velFollowOffset = velCommand-velFeedback;
    torqueFollowOffset =  torqueCommand-torqueFeedback;
    return 1;
}
int AxisObject::returnAxisCommandData(double &returnedPosition, double &returnedVel, double &returnedAcc, double &returnedTorque )
{
    returnedPosition=positionCommand;
    returnedVel=velCommand;
    returnedAcc=accCommand;
    returnedTorque=torqueCommand;
    return 1;
}

int AxisObject::returnAxisFeedbackData(double &returnedPosition, double &returnedVel, double &returnedTorque )
{
    returnedPosition=positionFeedback;
    returnedVel=velFeedback;
    returnedTorque=torqueFeedback;
    return 1;
}

int AxisObject::returnAxisFollowOffsetData(double &returnedPosition, double &returnedVel,double &returnedTorque )
{
    returnedPosition=positionOffset;
    returnedVel=velFollowOffset;
    returnedTorque=torqueFollowOffset;
    return 1;
}

int AxisObject::loadAxisAttribute(JointAttribute axisAttributeIn)
{
        t1RatioLimit=axisAttributeIn.t1RatioLimit;
        coulombFriction=axisAttributeIn.coulombFriction;
        coupleFlag=axisAttributeIn.coupleFlag;
        coupleJoint=axisAttributeIn.coupleJoint;
        coupleReducer=axisAttributeIn.coupleReducer;
        for(int i=0;i<4;i++)
        {
            dhParameter[i]=axisAttributeIn.dhParameter[i];
        }
        dhType=axisAttributeIn.dhType;
        encoderResolution=axisAttributeIn.encoderResolution;
        pulsePerRound=axisAttributeIn.pulsePerRound;
        externAxisFlag=axisAttributeIn.externAxisFlag;
        gearRatio=axisAttributeIn.gearRatio;
        jointName=axisAttributeIn.jointName;
        mass=axisAttributeIn.mass;
        massCenter=axisAttributeIn.massCenter;
        inertiaMatrix=axisAttributeIn.inertiaMatrix;
        maxAccelation=axisAttributeIn.maxAccelation;
        maxPosition=axisAttributeIn.maxPosition;
        approachDistance=axisAttributeIn.approachDistance;
        maxTorque=axisAttributeIn.maxTorque;
        jogTorqueLimit=axisAttributeIn.jogTorqueLimit;
        maxVelocity=axisAttributeIn.maxVelocity;
        maxFeedbackVelocity=axisAttributeIn.maxFeedbackVelocity;
        minPosition=axisAttributeIn.minPosition;
        mobile=axisAttributeIn.mobile;
        positionOffset=axisAttributeIn.positionOffset;
        processFollowError=axisAttributeIn.processFollowError;
        processVelocityFollowError=axisAttributeIn.processVelocityFollowError;
        screwPitch=axisAttributeIn.screwPitch;
        targetFollowError=axisAttributeIn.targetFollowError;
        viscousFriction=axisAttributeIn.viscousFriction;
        axisLogicalType=axisAttributeIn.axisLogicalType;
        return 1;
}
int AxisObject::setTimeAxisScale(double ratio)
{
    if(0>ratio || 1<ratio)
    {
        return -1;
    }
    else
    {
    timeAxisScale=ratio;
    }
    return 1;
}

int AxisObject::setDeltaTime(double time)
{
    deltaTime=time;
    return 1;
}

int AxisObject::moveOneStep()
{
    if(isTrajComposeEmpty())
    {
        return -1;
    }

    int limitCheck=-1;
    double tmpDuration=getTrajComposite()->Duration();
    if( isnan(tmpDuration)|| fabs(tmpDuration) <= 1e-6)//tmpDuration<deltaTime ||
    {
//         qDebug()<<"tmpDuration"<<tmpDuration;
//         addMsg(ENUM_MSG_ERROR,"MotionServer","AxisObject",5027,robotId, axisIdByRobot);
//        qDebug()<<"warnning,, AxisObject tmpDuration<deltaTime";
        clearTraj();
        return -5;//不要报错。
    }

    double accumulatedTime=getAccumulatedTime();

    if(accumulatedTime <=tmpDuration && accumulatedTime>=0.0)
    {
//        if(accumulatedTime <= tmpDuration  && accumulatedTime >tmpDuration -3*deltaTime)
//        {
//             qDebug()<<"vel end point"<<velCommand <<  "axis ID:"<< axisIdByRobot;
//        }

        accCommand= timeAxisScale*timeAxisScale*getTrajComposite()->Acc(accumulatedTime);
//        velCommand=timeAxisScale*velProfile->Vel(accumulatedTime);//关节手动，会自动生成速度命令，导致虽然是ＡＧＶ轴，但是关节手动会动。
        double tmppositionCommand= getTrajComposite()->Pos(accumulatedTime);
        if(isnan(tmppositionCommand))
        {
            tmppositionCommand=positionFeedback;
            qDebug()<<"error,isnan(tmppositionCommand)"<<tmppositionCommand;
            return -100;//不要报错。
        }
        limitCheck=moveOneStepByPosition(tmppositionCommand);
//        qDebug()<<"positionCommand"<<positionCommand;


        if(accumulatedTime == tmpDuration)
        {
            clearTraj();

            return limitCheck;

        }


    }
    else
    {
//        qDebug()<<"out accumulatedTime"<<accumulatedTime<<tmpDuration<<positionCommand;
        return -3;
    }
    //更新time
    accumulatedTime=accumulatedTimeMoveOneStep();
    //为了精确到达，后面可能要取消, agv的deltatime太大，不精确到达会导致时间轴偶尔减小不到０．
    if(accumulatedTime > tmpDuration &&
            accumulatedTime < tmpDuration+deltaTime )
    {
        setAccumulatedTime(tmpDuration,0);
//        qDebug()<<"accumulatedTime < tmpDuration+deltaTime"<<accumulatedTime<<tmpDuration<<positionCommand;

    }

    return limitCheck;

}

int AxisObject::moveOneStepByTrajIndex(int trajIndex)
{
    if(isTrajComposeEmpty())
    {
        positionCommand_cnt=positionCommand;
        return 0;
    }

//    int limitCheck=-1;
    double tmpDuration;
    double accumulatedTime=getAccumulatedTime(trajIndex);
    VelocityProfile * tmpTraj;
    tmpTraj=getTrajComposite(trajIndex);
    tmpDuration=tmpTraj->Duration();


    if( isnan(tmpDuration) || fabs(tmpDuration) <= 1e-6)//tmpDuration<deltaTime ||
    {
//         qDebug()<<"tmpDuration"<<tmpDuration;
//         addMsg(ENUM_MSG_ERROR,"MotionServer","AxisObject",5027,robotId, axisIdByRobot);
//        qDebug()<<"warnning,, AxisObject tmpDuration<deltaTime";
//        clearTraj();
        setAccumulatedTime(tmpDuration,trajIndex);
        if(D_DEBUG_AXIS_POSITION)
        {
    //        clearTraj();
            qDebug()<<"moveOneStepByTrajIndex return1 AXIS"<<axisIdByRobot<<"accumulatedTime "
                   <<accumulatedTime<<"Duration "<<tmpDuration<<"pos"<<positionCommand_cnt
                  <<"trajIndex"<<trajIndex;
        }
        return 0;//不要报错。
    }


    if(accumulatedTime <=tmpDuration && accumulatedTime>=0.0)
    {


        accCommand= timeAxisScale*timeAxisScale*tmpTraj->Acc(accumulatedTime);
        if(0)
        {
            velCommand=velCommand_byIncrement;
        }

//        velCommand=timeAxisScale*velProfile->Vel(*ppaccumulatedTime);//关节手动，会自动生成速度命令，导致虽然是ＡＧＶ轴，但是关节手动会动。
        positionCommand_cnt= tmpTraj->Pos(accumulatedTime);
        if(isnan(positionCommand_cnt))
        {
            positionCommand_cnt=positionFeedback;
            qDebug()<<"error,isnan(positionCommand_cnt)"<<positionCommand_cnt;
            return -100;//不要报错。
        }
//        limitCheck=moveOneStepByPosition(tmppositionCommand);
//        qDebug()<<"positionCommand"<<positionCommand;


        if(D_DEBUG_AXIS_POSITION)
        {
    //        clearTraj();
            qDebug()<<"moveOneStepByTrajIndex AXIS"<<axisIdByRobot<<"accumulatedTime "
                   <<accumulatedTime<<"Duration "<<tmpDuration<<"pos"<<positionCommand_cnt
                  <<"trajIndex"<<trajIndex;
        }

        if(accumulatedTime == tmpDuration)
        {
//            clearTraj();

//            return 1;

        }


    }
    else
    {
        if(D_DEBUG_AXIS_POSITION)
        {
    //        clearTraj();
            qDebug()<<"moveOneStepByTrajIndex return2 AXIS"<<axisIdByRobot<<"accumulatedTime "
                   <<accumulatedTime<<"Duration "<<tmpDuration<<"pos"<<positionCommand_cnt
                  <<"trajIndex"<<trajIndex;
        }
        return 0;
    }
    //更新time
    accumulatedTimeMoveOneStep(trajIndex);
    //为了精确到达，后面可能要取消, agv的deltatime太大，不精确到达会导致时间轴偶尔减小不到０．
    if(accumulatedTime > tmpDuration &&
            accumulatedTime < tmpDuration+deltaTime )
    {
         setAccumulatedTime(tmpDuration,trajIndex);

    }

    return 1;
}

int AxisObject::setVelCompensation(double velIn)
{
//    qDebug()<<"AxisObject::setVelCompensation axis"<<axisIdByRobot<<velIn;
    velCompensation=velIn;
    return isVelCompensationCheckOk(velIn);
}

int AxisObject::setTorqueCompensation(double torqueIn)
{
    torqueCompensation=torqueIn;
    return isTorqueCompensationCheckOk(torqueIn);
}

int AxisObject::moveOneStepByPosition(double positionIn)
{
//    if(false==isTrajComposeEmpty())//摆弧叠加要求不能判断。
//    {
//        return -1;
//    }
    if(E_MIX_STAGE_NONE==mixControlInfo.stage)
    {

        positionCommand=positionIn;
//        positionCommand=filteringAlgorithm_position->filterByForwardVel(positionIn,0.01);
    }
    else if(E_MIX_STAGE_READY==mixControlInfo.stage)
    {
        mixControlInfo.beginDuration=mixControlInfo.segmentDurationLeft;
        mixControlInfo.stage=E_MIX_STAGE_MIXING;
    }
    else if(E_MIX_STAGE_MIXING==mixControlInfo.stage)
    {

        moveOneStepByPosition_withMixerPre(positionIn,
                                        1-mixControlInfo.segmentDurationLeft/mixControlInfo.beginDuration);
        if(mixControlInfo.segmentDurationLeft<=deltaTime)
        {
            mixControlInfo.stage=E_MIX_STAGE_NONE;
        }

    }
    if(D_DEBUG_AXIS_POSITION)
    {
        qDebug()<<"最终位置 axisId"<<axisIdByRobot<<"mixControlInfo.stage"<<mixControlInfo.stage
               <<"positionCommand"<<positionCommand<<"positionIn"<<positionIn
                 <<"segmentDurationLeft"<<mixControlInfo.segmentDurationLeft<<"beginDuration"<<mixControlInfo.beginDuration;
    }

    return isLimitCheckOk();
}

int AxisObject::moveOneStepByPosition_plus(double positionPlus)
{
    return moveOneStepByPosition(positionCommand+positionPlus);
}

int AxisObject::moveOneStepByPosition_prepareMix(double positionIn, double durationLeftIn)
{
    if(E_MIX_STAGE_NONE==mixControlInfo.stage)
    {
        mixControlInfo.stage=E_MIX_STAGE_READY;
        positionCommand=positionIn;
        positionCommand_prepare=positionIn;
    }
    else if(E_MIX_STAGE_READY==mixControlInfo.stage)
    {
        positionCommand=positionIn;
        positionCommand_prepare=positionIn;
    }
    else if(E_MIX_STAGE_MIXING==mixControlInfo.stage)
    {
        positionCommand_prepare=positionIn;
    }
    if(mixControlInfo.segmentDurationLeft<=deltaTime)
    {
        mixControlInfo.stage=E_MIX_STAGE_NONE;
    }
    mixControlInfo.segmentDurationLeft=durationLeftIn;
    if(D_DEBUG_AXIS_POSITION)
    {
        qDebug()<<"moveOneStepByPosition_prepareMix混合准备位置 axisId"<<axisIdByRobot
               <<"positionIn"<<positionIn<<"durationLeftIn"<<durationLeftIn;
    }
    return isLimitCheckOk();

}

int AxisObject::moveOneStepByPosition_overlay(double positionIn)
{
    if(D_DEBUG_AXIS_POSITION)
    {
        qDebug()<<"moveOneStepByPosition_overlay叠加 axisId"<<axisIdByRobot<<"positionIn"<<positionIn;
    }
    positionCommand=(positionIn+positionCommand)/2.0;
    return isLimitCheckOk();
}

int AxisObject::moveOneStepByPosition_withMixer(double positionIn1,double positionIn2,
                                                double mixRatioIn)
{

    double tmpRatio=0.5+0.5*sin(mixRatioIn*M_PI-M_PI_2);
//    positionCommand=positionIn*mixRatioIn+positionCommand*(1-mixRatioIn);
    double tmpPositionCombined=positionIn2*tmpRatio+positionIn1*(1-tmpRatio);
    if(D_DEBUG_AXIS_POSITION)
    {
        qDebug()<<"moveOneStepByPosition_withMixer cnt混合 axisId"<<axisIdByRobot
               <<"mixRatioIn"<<mixRatioIn<<"positionIn1"<<positionIn1
              <<"positionIn2"<<positionIn2<<"tmpPositionCombined"<<tmpPositionCombined;
    }
    return moveOneStepByPosition(tmpPositionCombined);
}

int AxisObject::moveOneStepByPosition_withMixerPre(double positionIn,  double mixRatioIn)
{
    double tmpOld=positionCommand_prepare;

    double tmpRatio=0.5+0.5*sin(mixRatioIn*M_PI-M_PI_2);
//    positionCommand=positionIn*mixRatioIn+positionCommand*(1-mixRatioIn);
    positionCommand=positionIn*tmpRatio+positionCommand_prepare*(1-tmpRatio);
    if(D_DEBUG_AXIS_POSITION)
    {
        qDebug()<<"moveOneStepByPosition_withMixerPre混合计算 axisId"<<axisIdByRobot
               <<"mixRatioIn"<<mixRatioIn<<"positionIn"<<positionIn
              <<"tmpOld"<<tmpOld<<"tmpNew"<<positionCommand<<"tmpRatio"<<tmpRatio;
    }
    return isLimitCheckOk();
}

int AxisObject::moveOneStepByPosition_vel(double velIn)
{
    if(false==isTrajComposeEmpty())
    {
        return -1;
    }
    positionCommand+=velIn*deltaTime;

    if(D_DEBUG_AXIS_POSITION)
    {
        qDebug()<<"moveOneStepByPosition_vel axisId"<<axisIdByRobot<<"velIn"<<velIn;

    }

    return isLimitCheckOk();
}

int AxisObject::moveOneStepByPV(double positionIn, double velIn )
{
    if(false==isTrajComposeEmpty())
    {
        return -1;

    }
    //for agv ,this need to filtering

    positionCommand=positionIn;
    velCommand=velIn;
    if(1)
    {
        qDebug()<<"moveOneStepByPV axisId"<<axisIdByRobot<<"velIn"<<velIn;
    }


    return isLimitCheckOk();
}


int AxisObject::moveOneStepByV(double velIn)
{
    if(false==isTrajComposeEmpty())
    {
        qDebug()<<"failed, moveOneStepByV false==isTrajComposeEmpty() velIn"<<velIn;
        return -1;

    }
    //for agv ,this need to filtering
    velCommand=velIn;

    return isLimitCheckOk();
}

int AxisObject::moveOneStepByV_filter(double velIn)
{
    if(false==isTrajComposeEmpty())
    {
        return -1;

    }
    //for agv ,this need to filtering
    //对３个数据取平均值，会导致如果只发一次，可能和以前的抵消，导致速度为０.外部轴使用moveOneStepByPV保证不被抵消。
//    velCommand=filteringAlgorithm->arithmeticMeanfilter(velIn,3);//平均滤波.直线运动时，不是很平滑。
//    velCommand=filteringAlgorithm->middlevalueFilter(velIn,5);//中位值滤波
    velCommand=filteringAlgorithm->medianMean(velIn,6);//中位值平均滤波法
//    velCommand=velIn;

    return isLimitCheckOk();
}

int AxisObject::moveOneStepByPVAT(double positionIn, double velIn, double accIn, double torqueIn)
{
    if(false==isTrajComposeEmpty())
    {
        return -1;
    }
    positionCommand=positionIn;
    velCommand=velIn;
    accCommand=accIn;
    torqueCommand=torqueIn;

    qDebug()<<"moveOneStepByPVAT axisId"<<axisIdByRobot<<"velIn"<<velIn;

    return isLimitCheckOk();
}

int AxisObject::moveOneStepByTorque(double torqueIn)
{
    if(false==isTrajComposeEmpty())
    {
        return -1;
    }
    torqueCommand=torqueIn;

//    qDebug()<<"moveOneStepByTorque axisId"<<axisIdByRobot<<"velIn"<<velIn;

    return isLimitCheckOk();
}

int AxisObject::trapMove(double positionIn,double speedPercent,int cntIn)
{
    if(false==isTrajComposeEmpty())
    {
        return -1;
    }
    AxisTrajeAttribute tmpTrajAttri;
    tmpTrajAttri.trajComp= new VelocityProfile_Trap(maxVelocity*speedPercent, maxAccelation*axisAccelerationRatio);
    tmpTrajAttri.trajComp->SetProfile(positionCommand, positionIn);
//    accumulatedTime=0;
//    accumulatedTime_secondTraj=0;
    tmpTrajAttri.cnt=cntIn;
    tmpTrajAttri.timeDuration=tmpTrajAttri.trajComp->Duration();
    addNewTrajComposite(tmpTrajAttri);
//    qDebug()<<"trapMove positionIn"<<positionIn<<"duration"<<velProfile->Duration();
    return 1;
}

int AxisObject::trapMove(double positionIn,double speedPercent,double acceleration, int cntIn)
{
    if(false==isTrajComposeEmpty())
    {
        return -1;
    }
    AxisTrajeAttribute tmpTrajAttri;
    tmpTrajAttri.trajComp= new VelocityProfile_Trap(maxVelocity*speedPercent, acceleration*axisAccelerationRatio);
    tmpTrajAttri.trajComp->SetProfile(positionCommand, positionIn);
//    accumulatedTime=0;
//    accumulatedTime_secondTraj=0;
    tmpTrajAttri.cnt=cntIn;
    tmpTrajAttri.timeDuration=tmpTrajAttri.trajComp->Duration();
    addNewTrajComposite(tmpTrajAttri);
    return 1;
}


int AxisObject::trapMoveByTime(double positionIn, double timeDuration, double speedRatio, int cntIn)
{
//    if(false==isTrajComposeEmpty())
//    {
//        return -1;
//    }
    AxisTrajeAttribute tmpTrajAttri;
    tmpTrajAttri.trajComp= new VelocityProfile_Trap(maxVelocity*speedRatio, maxAccelation*axisAccelerationRatio);
    tmpTrajAttri.trajComp->SetProfileDuration(positionCommand_planed, positionIn,timeDuration);
//    accumulatedTime=0;
//    accumulatedTime_secondTraj=0;
    tmpTrajAttri.cnt=cntIn;
    tmpTrajAttri.timeDuration=tmpTrajAttri.trajComp->Duration();
    addNewTrajComposite(tmpTrajAttri);
    positionCommand_planed=positionIn;
    return 1;
}

int AxisObject::rectangularMove(double positionIn,double speedPercent, int cntIn)
{
    if(false==isTrajComposeEmpty())
    {
        return -1;
    }
    AxisTrajeAttribute tmpTrajAttri;
    tmpTrajAttri.trajComp= new VelocityProfile_Rectangular(maxVelocity*speedPercent);
    tmpTrajAttri.trajComp->SetProfile(positionCommand, positionIn);
//    accumulatedTime=0;
//    accumulatedTime_secondTraj=0;
    tmpTrajAttri.cnt=cntIn;
    tmpTrajAttri.timeDuration=tmpTrajAttri.trajComp->Duration();
    addNewTrajComposite(tmpTrajAttri);
    return 1;
}

int AxisObject::splineTapeCombinedMove(double positonIn, double speedPercent, int cntIn)
{
    //todo hualei
}

int AxisObject::splineZeroMove(double positionIn, double speedPercent, int cntIn)
{
    if(false==isTrajComposeEmpty())
    {
        return -1;
    }
    AxisTrajeAttribute tmpTrajAttri;
    tmpTrajAttri.trajComp= new VelocityProfile_Spline();
   double tmpTimeDuration=D_SPLINE_TIME_ESTIMATE_RATIO*fabs(positionCommand-positionIn)/(maxVelocity*speedPercent);
    ((VelocityProfile_Spline*)tmpTrajAttri.trajComp)->SetProfileDuration(positionCommand, 0.0, 0.0, positionIn, 0.0, 0.0, tmpTimeDuration);
//   accumulatedTime=0;
//   accumulatedTime_secondTraj=0;
   tmpTrajAttri.cnt=cntIn;
   tmpTrajAttri.timeDuration=tmpTrajAttri.trajComp->Duration();
   addNewTrajComposite(tmpTrajAttri);
    return 1;
}

int AxisObject::splineZeroMoveByTime(double positionIn,double timeDuration, int cntIn)
{
//    if(false==isTrajComposeEmpty())
//    {
//        return -1;
//    }
    AxisTrajeAttribute tmpTrajAttri;
    tmpTrajAttri.trajComp= new VelocityProfile_Spline();
    ((VelocityProfile_Spline*)tmpTrajAttri.trajComp)->SetProfileDuration(positionCommand_planed, 0.0, 0.0, positionIn, 0.0, 0.0, timeDuration);
    double tmpTimeDu=((VelocityProfile_Spline*)tmpTrajAttri.trajComp)->Duration();
//    accumulatedTime=0;
//    accumulatedTime_secondTraj=0;
    tmpTrajAttri.cnt=cntIn;
    tmpTrajAttri.timeDuration=tmpTrajAttri.trajComp->Duration();
    addNewTrajComposite(tmpTrajAttri);
    positionCommand_planed=positionIn;
    return 1;
}

int AxisObject::sinZeroMoveByTime(double positionIn, double timeDuration, int cntIn)
{
//    if(false==isTrajComposeEmpty())
//    {
//        return -1;
//    }
    AxisTrajeAttribute tmpTrajAttri;
    tmpTrajAttri.trajComp= new VelocityProfile_sin(maxVelocity,maxAccelation*axisAccelerationRatio);
    ((VelocityProfile_sin*)tmpTrajAttri.trajComp)->SetProfile_ZeroDuration(positionCommand_planed,  positionIn,  timeDuration);
//    double tmpTimeDu=((VelocityProfile_sin*)velProfile)->Duration();
//    accumulatedTime=0;
//    accumulatedTime_secondTraj=0;
    tmpTrajAttri.cnt=cntIn;
    tmpTrajAttri.timeDuration=tmpTrajAttri.trajComp->Duration();
    addNewTrajComposite(tmpTrajAttri);
    positionCommand_planed=positionIn;
    return 1;
}

int AxisObject::improvedSplineZeroMoveByTime(double positionIn, double timeDuration, int cntIn)
{
    if(false==isTrajComposeEmpty())
    {
        return -1;
    }
    AxisTrajeAttribute tmpTrajAttri;
    tmpTrajAttri.trajComp= new VelocityProfile_ImprovedSpline(maxVelocity,maxAccelation*axisAccelerationRatio);
    tmpTrajAttri.trajComp->SetProfileDuration(positionCommand,  positionIn, timeDuration);
//    double tmpTimeDu=tmpTrajAttri.trajComp->Duration();
//    accumulatedTime=0;
//    accumulatedTime_secondTraj=0;
    tmpTrajAttri.cnt=cntIn;
    tmpTrajAttri.timeDuration=tmpTrajAttri.trajComp->Duration();
    addNewTrajComposite(tmpTrajAttri);
    return 1;
}

int AxisObject::splineContinueMove(double positionIn,double velIn, double accIn, int cntIn)
{
    if(false==isTrajComposeEmpty())
    {
        return -1;
    }
    AxisTrajeAttribute tmpTrajAttri;
    tmpTrajAttri.trajComp= new VelocityProfile_Spline();
//    double tmpTimeDuration=4*fabs(positionCommand-positionIn)/fabs(velCommand+velIn);
    double tmpTimeDuration=4*fabs(positionCommand-positionIn)/maxVelocity;
    ((VelocityProfile_Spline*)tmpTrajAttri.trajComp)->SetProfileDuration(positionCommand, velCommand, accCommand, positionIn, velIn, accIn, tmpTimeDuration);
//    accumulatedTime=0;
//    accumulatedTime_secondTraj=0;
    tmpTrajAttri.cnt=cntIn;
    tmpTrajAttri.timeDuration=tmpTrajAttri.trajComp->Duration();
    addNewTrajComposite(tmpTrajAttri);
    return 1;
}



int AxisObject::splineContinueMoveByTime(double positionIn, double velIn, double accIn,
                                         double timeDuration, int cntIn)
{
    if(false==isTrajComposeEmpty())
    {
        return -1;
    }
    AxisTrajeAttribute tmpTrajAttri;
    tmpTrajAttri.trajComp= new VelocityProfile_Spline();
    //五次样条曲线
    ((VelocityProfile_Spline*)tmpTrajAttri.trajComp)->SetProfileDuration(positionCommand, velCommand, accCommand
                                                              , positionIn, velIn, accIn, timeDuration);
   //３次样条曲线
    //((VelocityProfile_Spline*)velProfile)->SetProfileDuration(positionCommand, velCommand
    //                                                          , positionIn, velIn, timeDuration);

//    accumulatedTime=0;
//    accumulatedTime_secondTraj=0;
    tmpTrajAttri.cnt=cntIn;
    tmpTrajAttri.timeDuration=tmpTrajAttri.trajComp->Duration();
    addNewTrajComposite(tmpTrajAttri);
    return 1;
}

int AxisObject::splineContinueMoveMultiPoint(QVector<VelPoint> velPoint)
{
    return 0;
//    int pointCount=velPoint.size();
//    if(pointCount<1 || pointCount>VEL_PROFILE_ARRY_MAX)
//    {
//        return -1;
//    }
//    for(int i=0;i<pointCount;i++)
//    {
//        AxisTrajeAttribute tmpTrajAttri;
//        tmpTrajAttri.trajComp= new VelocityProfile_Spline();
//        if(0==i)
//        {
//             ((VelocityProfile_Spline*)tmpTrajAttri.trajComp)->SetProfileDuration(positionCommand, velCommand, accCommand
//                            , velPoint[i].positionIn, velPoint[i].velIn, velPoint[i].accIn, velPoint[i].timeDuration);
////            qDebug()<<" vel asix id="<<axisIdByRobot<<"point id="<<i+1 <<"data"<<velCommand << velPoint[i].velIn;
//        }
//        else if(i>0)
//        {
//            ((VelocityProfile_Spline*)tmpTrajAttri.trajComp)->SetProfileDuration(velPoint[i-1].positionIn, velPoint[i-1].velIn, velPoint[i-1].accIn
//                           , velPoint[i].positionIn, velPoint[i].velIn, velPoint[i].accIn, velPoint[i].timeDuration);
////            qDebug()<<" vel asix id="<<axisIdByRobot<<"point id="<<i+1 <<"data"<< velPoint[i-1].velIn <<  velPoint[i].velIn;
//        }
//        velProfileArry[pointCount-1-i]=velProfile;
//        timeDurationCache.append(velProfile->Duration());
//        velProfile=NULL;// 不为空会导致无法添加新velProfile
//        currentVelProfileArryIndex=i;

//    }

//    velProfile=velProfileArry[pointCount-1];//初始化，准备给调用生点,最后索引开始


    return 1;
}

int AxisObject::getTimeDurationTrapMove(double positionIn,double speedPercent, double& timeDuration)
{
    if(speedPercent<0.001)
    {
        addMsg(ENUM_MSG_ERROR,"MotionServer","AxisObject",5011,robotId, axisIdByRobot);
        return -1;
    }
    VelocityProfile* tmpVelProfile= new VelocityProfile_Trap(maxVelocity*speedPercent, maxAccelation*axisAccelerationRatio);
    tmpVelProfile->SetProfile(positionCommand_planed, positionIn);
    timeDuration=tmpVelProfile->Duration();
    delete tmpVelProfile;
//    accumulatedTime=0;
    return 1;
}

int AxisObject::getTimeDurationSinMove(double positionIn, double speedPercent, double &timeDuration)
{
    if(speedPercent<0.001)
    {
        addMsg(ENUM_MSG_ERROR,"MotionServer","AxisObject",5011,robotId, axisIdByRobot);
        return -1;
    }
    VelocityProfile* tmpVelProfile= new VelocityProfile_sin(maxVelocity*speedPercent, maxAccelation*axisAccelerationRatio);
    tmpVelProfile->SetProfile(positionCommand_planed, positionIn);
    timeDuration=tmpVelProfile->Duration();
    delete tmpVelProfile;
//    accumulatedTime=0;
    return 1;
}

int AxisObject::getTimeDurationSplineZeroMove(double positionIn,double speedPercent, double& timeDuration)
{

    if(speedPercent<0.001)
    {
        addMsg(ENUM_MSG_ERROR,"MotionServer","AxisObject",5011,robotId, axisIdByRobot);
        return -1;
    }

    VelocityProfile* tmpVelProfile= new VelocityProfile_Spline();
    double tmpTimeDuration=D_SPLINE_TIME_ESTIMATE_RATIO*fabs(positionCommand_planed-positionIn)/(maxVelocity*speedPercent);
    ((VelocityProfile_Spline*)tmpVelProfile)->SetProfileDuration(positionCommand_planed, 0.0, 0.0, positionIn, 0.0, 0.0, tmpTimeDuration);
    timeDuration=tmpVelProfile->Duration();
    delete tmpVelProfile;
//    accumulatedTime=0;

    //for debug
    timeDuration=tmpTimeDuration;
//    qDebug()<<"AxisObject::getTimeDurationSplineZeroMove";
//    qDebug()<<"positionCommand="<<positionCommand<<"positionIn="<<positionIn<<"maxVelocity="<<maxVelocity
//           <<"speedPercent="<<speedPercent<<"tmpTimeDuration="<<tmpTimeDuration<<"timeDuration="<<timeDuration;

    return 1;

}

int AxisObject::getTimeDurationImprovedSplineZeroMove(double positionIn, double speedPercent, double &timeDuration)
{
    if(speedPercent<0.001)
    {
        addMsg(ENUM_MSG_ERROR,"MotionServer","AxisObject",5011,robotId, axisIdByRobot);
        return -1;
    }

    VelocityProfile* tmpVelProfile= new VelocityProfile_ImprovedSpline(maxVelocity*speedPercent,
                                                                       maxAccelation*axisAccelerationRatio);
    tmpVelProfile->SetProfile(positionCommand,  positionIn);
    timeDuration=tmpVelProfile->Duration();
    delete tmpVelProfile;
//    accumulatedTime=0;

    //for debug
//    timeDuration=tmpTimeDuration;
//    qDebug()<<"AxisObject::getTimeDurationSplineZeroMove";
//    qDebug()<<"positionCommand="<<positionCommand<<"positionIn="<<positionIn<<"maxVelocity="<<maxVelocity
//           <<"speedPercent="<<speedPercent<<"tmpTimeDuration="<<tmpTimeDuration<<"timeDuration="<<timeDuration;

    return 1;
}

int AxisObject::getTimeDurationSplineContinueMove(double positionIn,double velIn, double& timeDuration)
{
    VelocityProfile* tmpVelProfile= new VelocityProfile_Spline();
    //TODO 时间怎么估算最合理？当开始和结束时间为０时，有问题。
//    double tmpTimeDuration=4*fabs(positionCommand-positionIn)/fabs(velCommand+velIn);
    if(velIn<0.001)
    {
        addMsg(ENUM_MSG_ERROR,"MotionServer","AxisObject",5011,robotId, axisIdByRobot);
        return -1;
    }
    double tmpTimeDuration=4*fabs(positionCommand-positionIn)/(maxVelocity*velIn);
    ((VelocityProfile_Spline*)tmpVelProfile)->SetProfileDuration(positionCommand, 0.0, 0.0, positionIn, 0.0, 0.0, tmpTimeDuration);
    timeDuration=tmpVelProfile->Duration();
    delete tmpVelProfile;//单纯调用析构函数，内存不会被自动释放。
//    accumulatedTime=0;
    return 1;
}
 int AxisObject::waitPositionFollowOffset()
 {
     do
     {
         calculateFollowOffset();
     }
     while(fabs(positionFollowOffset)>targetFollowError);

     return 1;

 }



 int AxisObject::isPositionCommandCheckOk(double positionIn)
 {
     if(isnan(positionIn))
     {
         qDebug()<< "xxxxxxxxxxxxxxxxxx axid= "<< axisIdByRobot<<"  isnan"
                 <<positionIn;
         isPositionCommandExceed=2;
         addMsg(ENUM_MSG_ERROR,"MotionServer","AxisObject",5032,robotId, axisIdByRobot,positionIn);
        return -1;
     }

     if((positionIn > maxPosition) && ( positionIn >positionCommandOld) )
     {

          qDebug()<< "xxxxxxxxxxxxxxxxxx axid= "<< axisIdByRobot<<"  exceed maxPosition"
                  <<maxPosition<<"positionIn"<<positionIn<<"positionCommandOld"<<positionCommandOld;

          isPositionCommandExceed=2;
          addMsg(ENUM_MSG_ERROR,"MotionServer","AxisObject",5001,robotId, axisIdByRobot,positionIn);
         return -1;
     }
     else if( (positionIn < minPosition) && ( positionIn <positionCommandOld) )
     {

         qDebug()<< "xxxxxxxxxxxxxxxxxx axid= "<< axisIdByRobot<<"  exceed minPosition"
                 <<minPosition<<"positionIn"<<positionIn<<"positionCommandOld"<<positionCommandOld;;

         isPositionCommandExceed=1;
         addMsg(ENUM_MSG_ERROR,"MotionServer","AxisObject",5002,robotId, axisIdByRobot,positionIn);
         basicStatus=E_AXIS_STATUS_ERROR;
         return -1;
     }

     return 1;
 }

 int AxisObject::isPositionFeedbackCheckOk(double positionIn)
 {
     //反馈的位置会有波动，会导致超过限位后无法恢复到正常区间。需要连续判断多个周期才能下结论。

     if((positionIn > maxPosition) && ( positionIn >positionFeedbackOld) )//允许超过限位后，另外方向继续运行。
     {
         maxErrorCount++;

     }
     else if( (positionIn < minPosition) && ( positionIn <positionFeedbackOld) )
     {
         minErrorCount++;

     }
     else
     {
         maxErrorCount=0;
         minErrorCount=0;
     }

     if(maxErrorCount>10)
     {
         if(IF_DEBUG)
         {
             qDebug()<< "xxxxxxxxxxxxxxxxxx axid= "<< axisIdByRobot<<"  exceed maxPosition";
         }
          isPositionCommandExceed=2;
          addMsg(ENUM_MSG_ERROR,"MotionServer","AxisObject",5020,robotId, axisIdByRobot);
         return -1;
     }

     if(minErrorCount>10)
     {
         if(IF_DEBUG)
         {
             qDebug()<< "xxxxxxxxxxxxxxxxxx axid= "<< axisIdByRobot<<"  exceed minPosition";
         }
         isPositionCommandExceed=1;
         addMsg(ENUM_MSG_ERROR,"MotionServer","AxisObject",5021,robotId, axisIdByRobot);
         basicStatus=E_AXIS_STATUS_ERROR;
         return -1;
     }
     return 1;
 }

 int AxisObject::isPositionCommandIncrementCheckOk()
 {
//     return 1;//test888

     if( E_CHECK_MODE_T1 == limitCheckMode )
     {
         if( fabs(velCommand_byIncrement) > maxVelocity*t1RatioLimit*1.1 )//延迟一个周期velCommand_byIncrement才会被计算出来。
         {
             addMsg(ENUM_MSG_ERROR,"MotionServer","AxisObject",5016,robotId, axisIdByRobot,
                    velCommand_byIncrement,maxVelocity*t1RatioLimit*1.1 );
             if(IF_DEBUG)
             {
                 qDebug()<< "xxxxxxxxxxxxxxxxxx axid= "<< axisIdByRobot<<"  exceed maxVelocity";
             }
              isVelCommandExceed=1;
              basicStatus=E_AXIS_STATUS_ERROR;
             return -1;
         }
     }

     if( E_CHECK_MODE_T2 == limitCheckMode )
     {
         if( fabs(velCommand_byIncrement) > maxVelocity*1.2  )
         {
             addMsg(ENUM_MSG_ERROR,"MotionServer","AxisObject",5024,robotId, axisIdByRobot,
                                                velCommand_byIncrement,maxVelocity*1.2);
             if(1)//30000轴出现过１次，无法清除和复位，重启ok
             {
                 qDebug()<< "xxxxxxxxxxxxxxxxxx axid= "<< axisIdByRobot<<"  exceed maxVelocity"
                         <<maxVelocity<<"velCommand_byIncrement"<<velCommand_byIncrement
                         <<"positionCommand"<<positionCommand<<"positionCommandOld"<<positionCommandOld<<"deltaTime"<<deltaTime;
             }
              isVelCommandExceed=1;
              basicStatus=E_AXIS_STATUS_ERROR;
             return -1;
         }
     }
     return 1;
 }

 int AxisObject::isVelocityFollowOffsetOk()
 {
     if(fabs(velCommand_byIncrement-velFeedback)>processVelocityFollowError)
     {
         addMsg(ENUM_MSG_ERROR,"MotionServer","AxisObject",5026,robotId, axisIdByRobot,velCommand_byIncrement-velFeedback,processVelocityFollowError);
         if(IF_DEBUG)
         {
             qDebug()<< "xxxxxxxxxxxxxxxxxx axid= "<< axisIdByRobot<<"  exceed isVelocityFollowOffsetOk";
         }
          isVelCommandExceed=1;
          basicStatus=E_AXIS_STATUS_ERROR;
         return -1;
     }
     return 1;
 }

 int AxisObject::isVelCommandCheckOk(double velIn,bool isCheckT1)
 {
     if( E_CHECK_MODE_T1 == limitCheckMode && true==isCheckT1)
     {
         if( fabs(velIn) > maxVelocity*t1RatioLimit*1.1 )
         {
             addMsg(ENUM_MSG_ERROR,"MotionServer","AxisObject",5007,robotId, axisIdByRobot,velIn,
                    maxVelocity*t1RatioLimit*1.1);
             if(IF_DEBUG)
             {
                 qDebug()<< "xxxxxxxxxxxxxxxxxx axid= "<< axisIdByRobot<<"  exceed maxVelocity";
             }
              isVelCommandExceed=1;
              basicStatus=E_AXIS_STATUS_ERROR;
             return -1;
         }
     }

     if( E_CHECK_MODE_T2 == limitCheckMode )
     {
         if( fabs(velIn) > maxVelocity  )
         {
             addMsg(ENUM_MSG_ERROR,"MotionServer","AxisObject",5008,robotId, axisIdByRobot,velIn,maxVelocity);
             if(IF_DEBUG)
             {
                 qDebug()<< "xxxxxxxxxxxxxxxxxx axid= "<< axisIdByRobot<<"  exceed maxVelocity";
             }
              isVelCommandExceed=1;
              basicStatus=E_AXIS_STATUS_ERROR;
             return -1;
         }
     }

     return 1;
 }

 int AxisObject::isVelFeedbackCheckOk(double velIn,bool isCheckT1)
 {
     if( E_CHECK_MODE_T1 == limitCheckMode && true==isCheckT1)
     {
         if( fabs(velIn) > maxFeedbackVelocity*t1RatioLimit )//agv 控制转轴由于更新速率低，导致速度震荡。
         {
             addMsg(ENUM_MSG_ERROR,"MotionServer","AxisObject",5018,robotId, axisIdByRobot,velIn,
                    maxFeedbackVelocity*t1RatioLimit);
             if(IF_DEBUG)
             {
                 qDebug()<< "xxxxxxxxxxxxxxxxxx axid= "<< axisIdByRobot<<"  exceed maxVelocity";
             }
              isVelCommandExceed=1;
              basicStatus=E_AXIS_STATUS_ERROR;
             return -1;
         }
     }

     if( E_CHECK_MODE_T2 == limitCheckMode )
     {
         if( fabs(velIn) > maxFeedbackVelocity  )
         {
             addMsg(ENUM_MSG_ERROR,"MotionServer","AxisObject",5019,robotId, axisIdByRobot,velIn,
                    maxFeedbackVelocity);
             if(IF_DEBUG)
             {
                 qDebug()<< "xxxxxxxxxxxxxxxxxx axid= "<< axisIdByRobot<<"  exceed maxVelocity";
             }
              isVelCommandExceed=1;
              basicStatus=E_AXIS_STATUS_ERROR;
             return -1;
         }
     }

     return 1;
 }

 int AxisObject::isVelFeedbackIncrementCheckOk(double velIn)
 {

     if(0)
     {
         double tmpAcc=fabs((velIn-vel_old)/deltaTime) ;
         if( tmpAcc > 5000*maxAccelation*axisAccelerationRatio  )
         {
             addMsg(ENUM_MSG_ERROR,"MotionServer","AxisObject",5025,robotId, axisIdByRobot,tmpAcc,
                    maxAccelation*axisAccelerationRatio);

                 qDebug()<< "xxxxxxxxxxxxxxxxxx axid= "<< axisIdByRobot<<"  exceed maxAccelation"
                         <<" velIn"<<velIn<<" vel_old"<<vel_old;

              isAccCommandExceed=1;
              basicStatus=E_AXIS_STATUS_ERROR;
              vel_old=velIn;
             return -1;
         }
     }

     vel_old=velIn;
     return 1;
 }

 int AxisObject::isVelCompensationCheckOk(double velIn)
 {

     if( fabs(velIn) > maxFeedbackVelocity  )
     {
         addMsg(ENUM_MSG_ERROR,"MotionServer","AxisObject",5029,robotId, axisIdByRobot,velIn,
                maxFeedbackVelocity);
         if(IF_DEBUG)
         {
             qDebug()<< "isVelCompensationCheckOk xxxxxxxxxxxxxxxxxx axid= "<< axisIdByRobot<<"  exceed maxVelocity";
         }
          isVelCommandExceed=1;
          basicStatus=E_AXIS_STATUS_ERROR;
         return -1;
     }


     return 1;
 }

 int AxisObject::isAccCommandCheckOk(double accIn)
 {
//     if( fabs(accIn) > maxAccelation )
//     {
//         addMsg(ENUM_MSG_ERROR,"MotionServer","AxisObject",5009,robotId, axisIdByRobot);
//         if(IF_DEBUG)
//         {
//             qDebug()<< "xxxxxxxxxxxxxxxxxx axid= "<< axisIdByRobot<<"  exceed maxAccelation";
//         }
//          isAccCommandExceed=1;
//          basicStatus=E_AXIS_STATUS_ERROR;
//         return -1;
//     }

    return 1;
 }

  int AxisObject::isTorqueFeedbackCheckOk(double torqueIn)
  {

      if( E_CHECK_MODE_T1 == limitCheckMode )
      {
          if( fabs(torqueIn) > jogTorqueLimit )
          {
              addMsg(ENUM_MSG_ERROR,"MotionServer","AxisObject",5022,robotId, axisIdByRobot,torqueIn,
                     jogTorqueLimit);
              if(IF_DEBUG)
              {
                  qDebug()<< "xxxxxxxxxxxxxxxxxx axid= "<< axisIdByRobot<<"  exceed jogTorqueLimit";
              }
              isTorqueCommandExceed=1;
              basicStatus=E_AXIS_STATUS_ERROR;
              return -1;
          }
      }
      if( E_CHECK_MODE_T2 == limitCheckMode )
      {
          if( fabs(torqueIn) > maxTorque )
          {
              addMsg(ENUM_MSG_ERROR,"MotionServer","AxisObject",5023,robotId, axisIdByRobot,torqueIn,
                     maxTorque);
              if(IF_DEBUG)
              {
                  qDebug()<< "xxxxxxxxxxxxxxxxxx axid= "<< axisIdByRobot<<"  exceed maxTorque";
              }
              isTorqueCommandExceed=1;
              basicStatus=E_AXIS_STATUS_ERROR;
              return -1;
          }
      }
     return 1;
  }

 int AxisObject::isTorqueCommandCheckOk(double torqueIn)
 {

     if( E_CHECK_MODE_T1 == limitCheckMode )
     {
         if( fabs(torqueIn) > jogTorqueLimit )
         {
             addMsg(ENUM_MSG_ERROR,"MotionServer","AxisObject",5010,robotId, axisIdByRobot,torqueIn,
                    jogTorqueLimit);
             if(IF_DEBUG)
             {
                 qDebug()<< "xxxxxxxxxxxxxxxxxx axid= "<< axisIdByRobot<<"  exceed jogTorqueLimit";
             }
             isTorqueCommandExceed=1;
             basicStatus=E_AXIS_STATUS_ERROR;
             return -1;
         }
     }
     if( E_CHECK_MODE_T2 == limitCheckMode )
     {
         if( fabs(torqueIn) > maxTorque )
         {
             addMsg(ENUM_MSG_ERROR,"MotionServer","AxisObject",5017,robotId, axisIdByRobot,torqueIn,
                    maxTorque);
             if(IF_DEBUG)
             {
                 qDebug()<< "xxxxxxxxxxxxxxxxxx axid= "<< axisIdByRobot<<"  exceed maxTorque";
             }
             isTorqueCommandExceed=1;
             basicStatus=E_AXIS_STATUS_ERROR;
             return -1;
         }
     }
     return 1;
 }

 int AxisObject::isTorqueCompensationCheckOk(double torqueIn)
 {

     if( fabs(torqueIn) > maxTorque )
     {
         addMsg(ENUM_MSG_ERROR,"MotionServer","AxisObject",5028,robotId, axisIdByRobot,torqueIn,
                maxTorque);
         if(IF_DEBUG)
         {
             qDebug()<< "isTorqueCompensationCheckOk xxxxxxxxxxxxxxxxxx axid= "<< axisIdByRobot<<"  exceed maxTorque";
         }
         isTorqueCommandExceed=1;
         basicStatus=E_AXIS_STATUS_ERROR;
         return -1;
     }
     return 1;
 }

 int AxisObject::isPositionFollowOffsetCheckOk()
 {
     if( fabs(positionFollowOffset) > processFollowError  )
     {
         addMsg(ENUM_MSG_ERROR,"MotionServer","AxisObject",5015,robotId, axisIdByRobot,positionFollowOffset,processFollowError);
         if(IF_DEBUG)
         {
             qDebug()<< "xxxxxxxxxxxxxxxxxx axid= "<< axisIdByRobot<<"  exceed processFollowError";
         }

         isProcessFollowErrorExceed=1;
         basicStatus=E_AXIS_STATUS_ERROR;
         return -1;
     }

    return 1;
 }

 int AxisObject::isTargetOffsetCheckOk()
 {
     if(E_AXIS_LOGIC_TYPE_AGV==axisLogicalType)
     {
        return 1;
     }
     positionFollowOffset = positionCommand-positionFeedback;
     if( fabs(positionFollowOffset) > targetFollowError  )
     {
         qDebug()<<"error,AxisObject::isTargetOffsetCheckOk axisId"<<axisIdByRobot
                <<"positionFollowOffset"<<positionFollowOffset;
         return -1;
     }
    return 1;
 }


int AxisObject::isLimitCheckOk()
{
    int tmpKey=isLimitCheckOk_logic();
    if(1!=tmpKey)
    {
        positionCommand=positionFeedback;
//        positionCommandOld=positionCommand;
        positionFeedbackOld=positionFeedback;
        velCommand=velFeedback;
    }


    return tmpKey;
}

int AxisObject::isLimitCheckOk_logic()
{
    if(E_CHECK_EMERGENCY_STOP==limitCheckMode)
    {
        return 1;
    }

    switch(axisLogicalType)
    {
    case E_AXIS_LOGIC_TYPE_AGV:
        {
            if(1==isVelCommandCheckOk(velCommand)
                    && 1==isAccCommandCheckOk(accCommand)
                    && 1==isTorqueCommandCheckOk(torqueCommand))
            {
               return 1;
            }
            else
            {
                return -201;
            }
        }
        break;
    case E_AXIS_LOGIC_TYPE_VIRTUAL_STAND_ALONE:
        {
               return 1;

        }
        break;
    case E_AXIS_LOGIC_TYPE_AGVSTEERING:
        {
            if(1==isPositionCommandCheckOk(positionCommand)
                    && 1==isVelCommandCheckOk(velCommand,false)
                    && 1==isAccCommandCheckOk(accCommand)
                    && 1==isTorqueCommandCheckOk(torqueCommand))//不检查isPositionCommandIncrementCheckOk，因为会突变。
            {
                return 1;
            }
            else
            {
                return -202;
            }

        }
        break;
    default:
    {
        if(1==isPositionCommandCheckOk(positionCommand)
                && 1==isVelCommandCheckOk(velCommand)
                && 1==isAccCommandCheckOk(accCommand)
                && 1==isTorqueCommandCheckOk(torqueCommand)
                && 1==isPositionCommandIncrementCheckOk())
        {
            return 1;
        }
        else
        {
            return -202;
        }
    }
    }

    return -100;
}

 int AxisObject::setCheckMode(E_CHECK_MODE modeIn)
 {
    limitCheckMode=modeIn;
     return 1;
 }

 double AxisObject::getAccumulatedTime(int trajIndexIn)
 {  

     if(trajAttributeQueue.size()<=trajIndexIn || trajIndexIn<0)
     {
         return -1;

     }
     else
     {
        return trajAttributeQueue[trajIndexIn].accumulatedTime;
     }
     return -1;

 }

 double AxisObject::accumulatedTimeMoveOneStep(int trajIndexIn)
 {

     if(trajAttributeQueue.size()<=trajIndexIn || trajIndexIn<0)
     {
         return -1;

     }
     else
     {
        trajAttributeQueue[trajIndexIn].accumulatedTime+=timeAxisScale*deltaTime;
        return trajAttributeQueue[trajIndexIn].accumulatedTime;
     }
     return -1;
 }

 int AxisObject::setAccumulatedTime(double timeIn, int trajIndexIn)
 {

     if(trajAttributeQueue.size()<=trajIndexIn || trajIndexIn<0)
     {
         return -1;

     }
     else
     {
        trajAttributeQueue[trajIndexIn].accumulatedTime=timeIn;
     }
     return 1;

 }

 int AxisObject::clearTraj()
 {
     positionCommand_planed=positionCommand;
//     accumulatedTime=0;
//     accumulatedTime_secondTraj=0;
     //trajComposite->~VelocityProfile();
     deleteTrajComposite();
     mixControlInfo.stage=E_MIX_STAGE_NONE;
     return 1;
//     accumulatedTime=0;
//     //velProfile->~VelocityProfile();
//     delete velProfile;
//     velProfile=NULL;

//     //更新速度缓存队列到当前
//     if(0<currentVelProfileArryIndex)
//     {
//         currentVelProfileArryIndex-=1;
//         velProfile=velProfileArry[currentVelProfileArryIndex];
//     }
//     else if(0==currentVelProfileArryIndex)
//     {
//         currentVelProfileArryIndex=-1;
//         timeDurationCache.clear();
//     }
//     return 1;
 }

 int AxisObject::clearTraj_front()
 {
     if(trajAttributeQueue.size()>0)
     {
//         if(trajAttributeQueue.front().cnt>0)
//         {
//             qDebug()<<"ax,accumulatedTime=accumulatedTime_secondTraj"<<accumulatedTime_secondTraj;
////             accumulatedTime=accumulatedTime_secondTraj;
////             accumulatedTime_secondTraj=0;
//////             positionCommand=positionCommand_mix;
////             velCommand=velCommand_mixed;
////             accCommand=accCommand_mixed;
//         }
//         else
//         {
//             accumulatedTime=0;
//             accumulatedTime_secondTraj=0;

//         }
         delete trajAttributeQueue.front().trajComp;
         trajAttributeQueue.dequeue();
         return 1;
     }
     return 0;
 }

 double AxisObject::getPositionCommand_cnt()
 {
     return positionCommand_cnt;
 }


 int AxisObject::addMsg(int messageLevel, string componentName, string messageType, int messageCode, int robotIdIn,
                                      int parameter1, int parameter2, int parameter3, int parameter4)
 {
     Message tmpMsg;
     tmpMsg.MessageLevel = messageLevel;
     tmpMsg.componentClassName = componentName;
     tmpMsg.messageCode = messageCode;
     tmpMsg.messageType = messageType;
     tmpMsg.robotId = robotIdIn;

     string infomationStr;

     switch(messageCode)
     {
     case 5032:
       {
          string str1;
          str1 = QObject::tr("机器人(ID:").toStdString() +
                  QString::number(robotIdIn).toStdString() +
                  QObject::tr(")第").toStdString() +
                  QString::number(parameter1+1).toStdString() +
                  QObject::tr("轴位置(%2)为无穷nan").arg(parameter2).toStdString();
          infomationStr = str1;
          break;
       }
     case 5031:
       {
          string str1;
          str1 = QObject::tr("机器人(ID:").toStdString() +
                  QString::number(robotIdIn).toStdString() +
                  QObject::tr(")第").toStdString() +
                  QString::number(parameter1+1).toStdString() +
                  QObject::tr("轴位置(%2)接近负限位").arg(parameter2).toStdString();
          infomationStr = str1;
          break;
       }
       case 5030:
       {
          string str1;
          str1 = QObject::tr("机器人(ID:").toStdString() +
                  QString::number(robotIdIn).toStdString() +
                  QObject::tr(")第").toStdString() +
                  QString::number(parameter1+1).toStdString() +
                  QObject::tr("轴位置(%2)接近正限位").arg(parameter2).toStdString();
          infomationStr = str1;
          break;
       }
     case 5029:
     {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")第").toStdString() +
                QString::number(parameter1+1).toStdString() +
                QObject::tr("轴速度补偿(%1)超出限制(%2)").arg(parameter2).arg(parameter3).toStdString();
        infomationStr = str1;
        break;
     }
     case 5028:
     {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")第").toStdString() +
                QString::number(parameter1+1).toStdString() +
                QObject::tr("轴力矩补偿超出最大限制力矩").toStdString();
        infomationStr = str1;
        break;
     }
     case 5027:
     {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")第").toStdString() +
                QString::number(parameter1+1).toStdString() +
                QObject::tr("轴位移时间无穷小错误").toStdString();
        infomationStr = str1;
        break;
     }
     case 5026:
     {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")第").toStdString() +
                QString::number(parameter1+1).toStdString() +
                QObject::tr("轴速度跟随误差(%1)超出最大限制(%2)").arg(parameter2).arg(parameter3).toStdString();
        infomationStr = str1;
        break;
     }
     case 5025:
     {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")第").toStdString() +
                QString::number(parameter1+1).toStdString() +
                QObject::tr("轴反馈加速度(%1)超出最大加速度限制(%2)").arg(parameter2).arg(parameter3).toStdString();
        infomationStr = str1;
        break;
     }
     case 5024:
     {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")第").toStdString() +
                QString::number(parameter1+1).toStdString() +
                QObject::tr("轴命令位置变化等效速度(%1)超出T2限制(%2)").arg(parameter2).arg(parameter3).toStdString();
        infomationStr = str1;
        break;
     }
     case 5023:
     {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")第").toStdString() +
                QString::number(parameter1+1).toStdString() +
                QObject::tr("轴反馈力矩(%1)超出最大限制力矩(%2)").arg(parameter2).arg(parameter3).toStdString();
        infomationStr = str1;
        break;
     }
     case 5022:
     {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")第").toStdString() +
                QString::number(parameter1+1).toStdString() +
                QObject::tr("轴反馈力矩(%1)超出T1限制力矩(%2)").arg(parameter2).arg(parameter3).toStdString();
        infomationStr = str1;
        break;
     }
     case 5021:
       {
          string str1;
          str1 = QObject::tr("机器人(ID:").toStdString() +
                  QString::number(robotIdIn).toStdString() +
                  QObject::tr(")第").toStdString() +
                  QString::number(parameter1+1).toStdString() +
                  QObject::tr("轴反馈位置超出负限位").toStdString();
          infomationStr = str1;
          break;
       }
       case 5020:
       {
          string str1;
          str1 = QObject::tr("机器人(ID:").toStdString() +
                  QString::number(robotIdIn).toStdString() +
                  QObject::tr(")第").toStdString() +
                  QString::number(parameter1+1).toStdString() +
                  QObject::tr("轴反馈位置超出正限位").toStdString();
          infomationStr = str1;
          break;
       }
     case 5019:
     {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")第").toStdString() +
                QString::number(parameter1+1).toStdString() +
                QObject::tr("轴反馈速度(%1)超出T2限制(%2)").arg(parameter2).arg(parameter3).toStdString();
        infomationStr = str1;
        break;
     }
     case 5018:
     {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")第").toStdString() +
                QString::number(parameter1+1).toStdString() +
                QObject::tr("轴反馈速度(%1)超出T1限制(%2)").arg(parameter2).arg(parameter3).toStdString();
        infomationStr = str1;
        break;
     }
     case 5017:
     {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")第").toStdString() +
                QString::number(parameter1+1).toStdString() +
                QObject::tr("轴力矩超出最大限制力矩").toStdString();
        infomationStr = str1;
        break;
     }

     case 5016:
     {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")第").toStdString() +
                QString::number(parameter1+1).toStdString() +
                QObject::tr("轴命令位置变化等效速度(%1)超出T1限制(%2)").arg(parameter2).arg(parameter3).toStdString();
        infomationStr = str1;
        break;
     }
     case 5015:
     {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")第").toStdString() +
                QString::number(parameter1+1).toStdString() +
                QObject::tr("轴过程跟随误差(%1)超出限制(%2)").arg(parameter2).arg(parameter3).toStdString();
        infomationStr = str1;
        break;
     }
     case 5011:
         {
            string str1;
            str1 = QObject::tr("机器人(ID:").toStdString() +
                    QString::number(robotIdIn).toStdString() +
                    QObject::tr(")第").toStdString() +
                    QString::number(parameter1+1).toStdString() +
                    QObject::tr("轴速度小于0.00１％，不合理！").toStdString();
            infomationStr = str1;
            break;
         }
         case 5010:
         {
            string str1;
            str1 = QObject::tr("机器人(ID:").toStdString() +
                    QString::number(robotIdIn).toStdString() +
                    QObject::tr(")第").toStdString() +
                    QString::number(parameter1+1).toStdString() +
                    QObject::tr("轴力矩超出T1限制力矩").toStdString();
            infomationStr = str1;
            break;
         }
         case 5009:
         {
            string str1;
            str1 = QObject::tr("机器人(ID:").toStdString() +
                    QString::number(robotIdIn).toStdString() +
                    QObject::tr(")第").toStdString() +
                    QString::number(parameter1+1).toStdString() +
                    QObject::tr("轴加速度超出限制").toStdString();
            infomationStr = str1;
            break;
         }
         case 5008:
         {
            string str1;
            str1 = QObject::tr("机器人(ID:").toStdString() +
                    QString::number(robotIdIn).toStdString() +
                    QObject::tr(")第").toStdString() +
                    QString::number(parameter1+1).toStdString() +
                    QObject::tr("轴命令速度(%1)超出T2限制(%2)").arg(parameter2).arg(parameter3).toStdString();
            infomationStr = str1;
            break;
         }
         case 5007:
         {
            string str1;
            str1 = QObject::tr("机器人(ID:").toStdString() +
                    QString::number(robotIdIn).toStdString() +
                    QObject::tr(")第").toStdString() +
                    QString::number(parameter1+1).toStdString() +
                    QObject::tr("轴速度超出T1限制").toStdString();
            infomationStr = str1;
            break;
         }
       case 5002:
         {
            string str1;
            str1 = QObject::tr("机器人(ID:").toStdString() +
                    QString::number(robotIdIn).toStdString() +
                    QObject::tr(")第").toStdString() +
                    QString::number(parameter1+1).toStdString() +
                    QObject::tr("轴命令位置(%2)超出负限位").arg(parameter2).toStdString();
            infomationStr = str1;
            break;
         }
         case 5001:
         {
            string str1;
            str1 = QObject::tr("机器人(ID:").toStdString() +
                    QString::number(robotIdIn).toStdString() +
                    QObject::tr(")第").toStdString() +
                    QString::number(parameter1+1).toStdString() +
                    QObject::tr("轴命令位置(%2)超出正限位").arg(parameter2).toStdString();
            infomationStr = str1;
            break;
         }





         default:
         {
             qDebug()<<"Coordinate::addMsg====not match error code";
             break;
         }
     }

     tmpMsg.MessageInformation = infomationStr;

     #ifndef MOTION_SERVER_UNIT_TEST
     motionMessage->addMsg(tmpMsg);
     #endif
 }
